/**
 * @file servo.c
 * @author Sevenfite (lin481399413@163.com)
 * @brief 
 * @version 0.1
 * @date 2024-08-05
 * 
 * @interface 
 * 接口函数：
 *  ServoInit() 舵机初始化,关于舵机的一些参数设置需要改变此函数源码
 *  setServoAngleByIndex(uint8_t servoNum, uint16_t angle) 通过index设置舵机角度
 * @note
 *  Servo结构体存储了舵机的信息，包括舵机编号，设置的角度，最大角度，模式（模拟或数字），
 * 以及对应的GPIO（数字模式下）或者TIM(模拟模式下)
 *  本地全局变量gl_servo[SERVO_NUM]存储了所有舵机的信息
 * @note 
 * 关于数字舵机和模拟舵机的区别：
 * 数字舵机：舵机内置芯片，记录角度信息，所以只用发一次PWM信号，
 * 通过GPIO控制，每次设置角度时，需要给舵机一个脉冲，脉冲的高电平时间决定了舵机的角度，一般来说一个脉冲就可以了，如果不行，可以增加
 * 模拟舵机：传统的舵机，需要不断的发PWM信号，通过TIM控制，每次设置角度时，需要不断的发PWM信号，直到舵机转到指定角度，
 * 定时器的参数，PWM频率为50Hz，周期为20ms，高电平时间为0.5ms-2.5ms，对应角度为0-180度
 * 
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include "servo.h"
#include "delay.h"
#include "tim.h"

enum ServoMode{
  ANALOG_SERVO, // 模拟舵机
  DIGITAL_SERVO // 数字舵机
};
struct Servo{
  uint8_t servoNum;
  uint16_t angle;
  uint16_t maxAngle;
  enum ServoMode mode; // 0:模拟舵机 1:数字舵机
  union
  {
    GPIO_TypeDef *GPIOx; // 数字舵机用
    TIM_HandleTypeDef *htim; //模拟舵机用
    /* data */
  }Info1;
  union 
  {
    uint32_t channel; // 模拟舵机用
    uint16_t pin; // 数字舵机用
    /* data */
  }Info2;
};

struct Servo gl_servo[SERVO_NUM];
static uint16_t calServoAngle(uint8_t angle, uint16_t maxAngle);
static void setServoAngle(struct Servo* servo, uint16_t angle);
static void setServo(struct Servo *servo,uint8_t servoNum, uint16_t angle, uint16_t maxAngle, enum ServoMode mode, void *Info1, uint32_t Info2){
  servo->servoNum = servoNum;
  servo->angle = angle;
  servo->maxAngle = maxAngle;
  servo->mode = mode;
  if(mode == DIGITAL_SERVO){
    servo->Info1.GPIOx = (GPIO_TypeDef *)Info1;
    servo->Info2.pin = (uint16_t)Info2;
  }else{
    servo->Info1.htim = (TIM_HandleTypeDef *)Info1;
    servo->Info2.channel = Info2;
  }
}
/**
 * @brief 舵机初始化函数
 * 
 */
void ServoInit(void)
{
  // 舵机初始化，设置舵机编号，初始角度，最大角度，模式，对应的GPIO或者TIM，如需更改舵机参数，需要改变这里的源码
  setServo(&gl_servo[0],1,0,180,ANALOG_SERVO,&SERVO_1_Tim,SERVO_1_Channel);
  setServo(&gl_servo[1],2,0,180,ANALOG_SERVO,&SERVO_2_Tim,SERVO_2_Channel);
  setServo(&gl_servo[2],3,0,180,ANALOG_SERVO,&SERVO_3_Tim,SERVO_3_Channel);
  setServo(&gl_servo[3],4,0,180,ANALOG_SERVO,&SERVO_4_Tim,SERVO_4_Channel);
  setServo(&gl_servo[4],5,0,180,DIGITAL_SERVO,SERVO_5_GPIO,SERVO_5_Pin);
  setServo(&gl_servo[5],6,0,180,DIGITAL_SERVO,SERVO_6_GPIO,SERVO_6_Pin);
  setServo(&gl_servo[6],7,0,180,DIGITAL_SERVO,SERVO_7_GPIO,SERVO_7_Pin);
  setServo(&gl_servo[7],8,0,180,DIGITAL_SERVO,SERVO_8_GPIO,SERVO_8_Pin);

  //初始化角度
  gl_servo[SERVO_BODY].angle=SERVO_BODY_INIT;
  gl_servo[SERVO_LEFT_ARM].angle=SERVO_LEFT_ARM_INIT;
  gl_servo[SERVO_RIGHT_ARM].angle=SERVO_RIGHT_ARM_INIT;
  gl_servo[SERVO_CAMERA].angle=SERVO_CAMERA_INIT;
  gl_servo[SERVO_COLOR].angle=SERVO_COLOR_INIT;

  for(int i = 0; i < SERVO_NUM; i++){
    if(gl_servo[i].mode == ANALOG_SERVO){
      HAL_TIM_PWM_Start(gl_servo[i].Info1.htim,gl_servo[i].Info2.channel);
    }
  }
  for(int i = 0; i < SERVO_NUM; i++){
    setServoAngle(&gl_servo[i],gl_servo[i].angle);
  }
}
/**
 * @brief Set the Servo Angle By Index object
 * 
 * @param servoNum 从0开始，小于SERVO_NUM
 * @param angle 要设置的角度
 */
void setServoAngleByIndex(uint8_t servoNum, uint16_t angle){
  setServoAngle(&gl_servo[servoNum],angle);
}
/**
 * @brief 设置舵机角度
 *
 * @param servoNum 舵机编号
 * @param angle 0-180度
 */
static void setServoAngle(struct Servo* servo, uint16_t angle) {
  if(angle > servo->maxAngle){
    angle = servo->maxAngle;
  }
  servo->angle = angle;
  if(servo->mode == DIGITAL_SERVO){
      HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_SET);
			delay_us(calServoAngle(servo->angle,servo->maxAngle));
			HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_RESET);
			delay_us(3000-calServoAngle(servo->angle,servo->maxAngle));
      HAL_Delay(17);
      HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_SET);
			delay_us(calServoAngle(servo->angle,servo->maxAngle));
			HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_RESET);
			delay_us(3000-calServoAngle(servo->angle,servo->maxAngle));
      HAL_Delay(17);
      HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_SET);
			delay_us(calServoAngle(servo->angle,servo->maxAngle));
			HAL_GPIO_WritePin(servo->Info1.GPIOx,servo->Info2.pin,GPIO_PIN_RESET);
			delay_us(3000-calServoAngle(servo->angle,servo->maxAngle));
  }
  else{
    __HAL_TIM_SET_COMPARE(servo->Info1.htim, servo->Info2.channel, calServoAngle(servo->angle,servo->maxAngle));
  }
}

/**
 * @brief 输入角度，返回500-2500
 * 
 * @param angle 角度0-180
 * @param maxAngle 最大角度
 * @return uint16_t 返回比较值500-2500
 */
static uint16_t calServoAngle(uint8_t angle, uint16_t maxAngle) {
  return angle *2000 /maxAngle + 500;
}

/*--------------------Test------------------*/
#include "ABS_key.h"
#include "display.h"
#include "myconfig.h"
/// @brief 舵机调试，0 45 90 135
/// @param key_Num
/// @return
My_StatusTypeDef servo(int key_Num) {
  static uint8_t servoNum = 0;
  static uint8_t flag = 0;
  if(flag == 0){
    displayStr(" servoNum:", Display_firstCol+Display_OneChar, Display_firstRow, RED);
    displayStr(" angle:", Display_firstCol+Display_OneChar, Display_thirdRow, RED);
    flag = 1;
  }
  displayNum(servoNum+1, Display_firstCol, Display_secondRow);
  displayNum(gl_servo[servoNum].angle, Display_firstCol, Display_fourthRow);
  if(flag == 1){
    displayStr(">", Display_firstCol, Display_firstRow, RED);
    displayStr(" ", Display_firstCol, Display_thirdRow, RED);
  }
  else if(flag ==2){
    displayStr(" ", Display_firstCol, Display_firstRow, RED);
    displayStr(">", Display_firstCol, Display_thirdRow, RED);
  }
  if (key_Num == Key_Down) {
    if(flag == 1){
      servoNum++;
      servoNum %= SERVO_NUM;
    }
    else if(flag == 2){
      uint16_t nowAngle = gl_servo[servoNum].angle;
      nowAngle+=10;
      nowAngle %= gl_servo[servoNum].maxAngle;
      setServoAngle(&gl_servo[servoNum],nowAngle);
    }
  } else if (key_Num == Key_UP) {
    if(flag == 1){
      servoNum+=SERVO_NUM-1;
      servoNum %= SERVO_NUM;
    }
    else if(flag == 2){
      uint16_t nowAngle = gl_servo[servoNum].angle;
      nowAngle+=gl_servo[servoNum].maxAngle-10;
      nowAngle %= gl_servo[servoNum].maxAngle;
      setServoAngle(&gl_servo[servoNum],nowAngle);
    }
  }
  else if (key_Num == Key_ESC) {
    flag = 0;
    return MY_OK;
  }
  else if (key_Num == Key_OK)
  {
    flag%=2;
    flag++;
  }
  return MY_BUSY;
}